#include <ros/ros.h>
#include"std_msgs/Float64MultiArray.h"
#include <stdio.h>
#include <termios.h>

int getch()
{ 
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt; 
  newt.c_lflag &= ~(ICANON);                 // disable buffering      
  newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings
   
  int c = getchar();  // read character (non-blocking)
  
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
 }

uint8_t key_input_time_x, key_input_time_y;

int main(int argc, char *args[])
{
    ros::init(argc, args, "key_remote");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Float64MultiArray>("robot_remote", 1);

    std_msgs::Float64MultiArray remote_msg;
    remote_msg.data.resize(3);

    ROS_INFO_STREAM("key remote start succeed!");

    ros::Rate rate(45);
    
    while (ros::ok())
    {
        uint8_t key = getch();

        if(key != 255) {
            if (key == 'w') {
                key_input_time_x = 5;
                remote_msg.data[0] = 100;
            }
            if (key == 's') {
                key_input_time_x = 5;
                remote_msg.data[0] = -100;
            }

            if (key == 'a') {
                key_input_time_y = 5;
                remote_msg.data[1] = 100;
            }
            if (key == 'd') {
                key_input_time_y = 5;
                remote_msg.data[1] = -100;
            }

        } else {
            if (key_input_time_x == 0) {
                remote_msg.data[0] = 0;
            } else {
                key_input_time_x--;
            }

            if (key_input_time_y == 0) {
                remote_msg.data[1] = 0;
            } else {
                key_input_time_y--;
            }
        }

        // ROS_INFO("c:%d, x:%f, y:%f, z:%f", key, remote_msg.data[0], remote_msg.data[1], remote_msg.data[2]);

        pub.publish(remote_msg);
        
        rate.sleep();
    }

    return 0;
}